!**********************************************************************

! Code converted using TO_F90 by Alan Miller
! Date: 2013-05-26  Time: 22:02:29

!************************ MODULES FOR ROCKET3 **************************
!**********************************************************************
!*** *
!*** * Calling sequence of Modules:
!*** *   G2   ENVIRONMENT
!*** *   A2   PROPULSION
!*** *   A1   AERODYNAMICS
!*** *   A3   FORCES
!*** *   C2   AUTOPILOT
!*** *   C4   ACTUATOR
!*** *   D1   NEWTONS LAW
!*** * with dummy RETURNs for unused modules
!*** *
!*** * MODIFICATION HISTORY
!*** * 000419 Version 1.0 Created by Peter Zipfel
!*** *
!**********************************************************************
!**********************************************************************

SUBROUTINE a1i
!**********************************************************************
!*** * Initialization of Aerodynamic Module A1.
!*** * Reserved C(3510) locations are 1200-1299
!*** * (1) Initializes placeholder index for table look-up
!*** *
!*** * MODIFICATION HISTORY
!*** * 000112 Created by Peter Zipfel
!*** *
!*** ******************************************************************

COMMON c(3510)

!*** INITIALIZATION

RETURN
END SUBROUTINE a1i
!**********************************************************************

SUBROUTINE a1
!**********************************************************************
!*** * The Aerodynamic Module A1.
!*** * Reserved C(3510) locations are 1200-1299
!*** *
!*** *  MAERO =|MAERT| |MAERV|   (Type,Vehicle)
!*** *
!*** *          MAERT=1 Test Ascent Rocket
!*** *                  MAERV=1 1.Stage
!*** *                       =2 2.Stage
!*** *                       =3 3.Stage
!*** *
!*** * This module performs the following functions:
!*** * (1) Provides  CL and CD as functions of Mach
!*** *
!*** * MODIFICATION HISTORY
!*** * 000112 Created by Peter Zipfel
!*** *
!*** ******************************************************************

COMMON c(3510)

!***  INPUT DATA

EQUIVALENCE (c(1200),maero)
EQUIVALENCE (c(1203),alphax)

! MAERO = D =|MAERT|MAERV|, MAERT=1:Type, MAERV:Stage #
! ALPHAX = D/O Angle of attack - deg

!*** INPUT FROM EXECUTIVE

EQUIVALENCE (c(0052),crad)

!*** INPUT FROM OTHER MODULES

EQUIVALENCE (c(0206),vmach)
EQUIVALENCE (c(1300),mprop)

! VMACH= O Mach number of rocket - ND
! MPROP= D/G =0:No thrust; =1:Coasting =2:Burning

!*** OUTPUT TO OTHER MODULES

EQUIVALENCE (c(1201),cd)
EQUIVALENCE (c(1202),cl)

! CD = O Drag coefficient - ND
! CL = O Lift coefficient - ND

!*** DIAGNOSTICS

EQUIVALENCE (c(1206),ca)
EQUIVALENCE (c(1207),cn)
EQUIVALENCE (c(1208),clovercd)

! CA = G Axial force coefficient - ND
! CN = G Normal force coefficient - ND
! CLOVERCD = G Lift over drag - ND

maert=INT(maero/10.)
maerv=maero-maert*10.
alpha=alphax/crad
calph=COS(alpha)
salph=SIN(alpha)

!*** ASCENT TEST ROCKET (STORM)

IF(maert == 1)THEN
  IF(maerv == 1)THEN
    
!***       FIRST STAGE
    
    IF(mprop == 2)THEN
      caa=.281+.186*vmach-.056*vmach**2+.00366*vmach**3
    ELSE
      caa=.346+.183*vmach-.058*vmach**2+.00382*vmach**3
    END IF
    cnn=(5.006-.519*vmach+.031*vmach**2)*alpha
  END IF
  
  IF(maerv == 2)THEN
    
!***       SECOND STAGE
    
    IF(mprop == 2)THEN
      caa=.236-.043*vmach+.0029*vmach**2-.00006*vmach**3
    ELSE
      caa=.327-.067*vmach+.005*vmach**2-.0001*vmach**3
    END IF
    cnn=(1.714-.038*vmach+.0014*vmach**2)*alpha
  END IF
  
  IF(maerv == 3)THEN
    
!***       THIRD STAGE
    
    caa=.02
    cnn=1*alpha
  END IF
  
!***    CONVERT COEFFICIENTS
  
  cdd=caa*calph+cnn*salph
  cll=cnn*calph-caa*salph
  cl=cll
  cd=cdd
  clovercd=cl/cd
  
  cn=cl*calph+cd*salph
  ca=cd*calph-cl*salph
  
END IF

RETURN
END SUBROUTINE a1
!**********************************************************************

SUBROUTINE a2i
!**********************************************************************
!*** * Propulsion Initialization.
!*** * Reserved C(3510) locations are 1300-1399
!*** *
!*** * * Fixed nozzle, variable fuel flow motor, ideal nozzle expansion
!*** * is assumed. Throttle ratio THRTL
!*** * governs the flow rate.
!*** * * Several stages; the total mass of the remaining stages is initialized
!*** * by VMASSI; new engine parameters may also be introduced at every
!*** * stage; the current mass is VMASS;
!*** *
!*** * MPROP =0 No ignition; or engine shut-down (D)
!*** *       =1 Missile is coasting after emgine shut-down.
!*** *          (shut-down occurs either at fuel burn-out or
!*** *           when MPROP=0 set in input (G)
!*** *       =2 Motor ignition; re-ignition; or motor burning (D)
!*** *
!*** * This module performs the following functions:
!*** *
!*** * (1) Initializes vehicle and fuel masses
!*** *
!*** * MODIFICATION HISTORY
!*** * 000113 Created by Peter Zipfel
!*** *
!*** ******************************************************************

COMMON c(3510)

!*** INPUT DATA

EQUIVALENCE (c(1305),vmassi)

! VMASSI = D Initial mass of remaining stages - kg

!*** INITIALIZATION

EQUIVALENCE (c(1308),fmassf)
EQUIVALENCE (c(1309),vmass)
EQUIVALENCE (c(1312),fmassfm)
EQUIVALENCE (c(1314),vmassim)

! FMASSF = I/G Expended fuel in stage - kg
! VMASS = I/O Vehicle mass - kg
! FMASSFM = I Expended fuel in stage, stored - kg
! VMASSIM = I Initial mass of prior stage - kg

fmassf=0.
vmass=vmassi
fmassfm=0.
vmassim=0.

RETURN
END SUBROUTINE a2i
!**********************************************************************

SUBROUTINE a2
!**********************************************************************
!*** * Propulsion.
!*** * Reserved C(3510) locations are 1300-1399
!*** *
!*** * This module performs the following functions:
!*** *
!*** * (1) Calculates missile thrust
!*** * (2) Initiates rocket booster staging
!*** * (3) Calculates missile mass
!*** *
!*** * MODIFICATION HISTORY
!*** * 000113 Created by Peter Zipfel
!*** *
!*** ******************************************************************

COMMON c(3510)

!*** INPUT DATA

EQUIVALENCE (c(1300),mprop)
EQUIVALENCE (c(1302),fueli)
EQUIVALENCE (c(1303),fuelr)
EQUIVALENCE (c(1304),spi)
EQUIVALENCE (c(1305),vmassi)
EQUIVALENCE (c(1310),thrtl)
EQUIVALENCE (c(1316),aexit)
EQUIVALENCE (c(1317),pexit)

! MPROP = D/G =0:No thrust; =1:Coasting =2:Burning
! FUELI = D Initial total fuel in stage - kg
! FUELR = D Maximum fuel rate through motor - kg/s
! SPI = D Effective specific impulse - s
! VMASSI = D Initial mass of remaining stages - kg
! THRTL = D Throttle (0-1) - ND
! AEXIT = D Nozzle exit aerea - m^2
! PEXIT = D Rocket nozzle exit pressure - Pa

!*** INIIALIZATION

EQUIVALENCE (c(1308),fmassf)
EQUIVALENCE (c(1312),fmassfm)
EQUIVALENCE (c(1314),vmassim)

! FMASSF = I/G Expended fuel in stage - kg
! FMASSFM = I Expended fuel in stage, stored - kg
! VMASSIM = I Initial mass of prior stage - kg

!*** INPUT FROM EXECUTIVE

EQUIVALENCE (c(0054),agrav)
EQUIVALENCE (c(2664),der)
EQUIVALENCE (c(2866),icoor)

! AGRAV = E Gravitational acceleration (refrence) - m/s^2

!*** INPUT FROM OTHER MODULES

EQUIVALENCE (c(0202),patm)

! PATM= O Atmospheric pressure - Pa

!*** OUTPUT TO OTHER MODULES

EQUIVALENCE (c(1313),thrustx)
EQUIVALENCE (c(1309),vmass)

! THRUSTX = O Thrust at altitude - kN
! VMASS = O Vehicle mass - kg

!*** DIAGNOSTICS

EQUIVALENCE (c(1306),fmassfd)
EQUIVALENCE (c(1311),fuel)

! FMASSFD = G Fuel rate - kg/s
! FUEL = G Fuel remaining in stage - kg

!*** FOR NEW STAGE: INITIALIZE EXPENDED FUEL TO ZERO

IF((vmassim-vmassi) > 0.) THEN
  fmassfm=0.
  fmassf=0.
  fmassfd=0.
END IF
vmassim=vmassi
!      PRINT *, "Propulsion"

!*** CALCULATE FUEL EXPENDED IN STAGE

IF(icoor == 0) fmassf=fmassf+fmassfd*der

IF(mprop == 0) THEN
  
!***     NO BURNING
  
  thrust=0.
  fmassfd=0.
  vmass=vmassi-fmassfm
  fuel=fueli-fmassfm
END IF

IF(mprop == 1) THEN
  
!***     COAST AFTER BURN-OUT
  
  thrust=0.
  fmassfd=0.
  fmassf=0.
  vmass=vmassi-fmassfm
  fuel=fueli-fmassfm
END IF

IF(mprop == 2) THEN
  
!***     ROCKET MOTOR BURNING
  
  flr=fuelr*thrtl
  
!-- Change this to use the Pe variable
!         THRUST=FLR*SPI*AGRAV+(101325.-PATM)*AEXIT
  thrust=flr*spi*agrav+(pexit-patm)*aexit
!         PRINT *, 'T to W:', THRUST / VMASS
  fmassfd=flr
  fuel=fueli-fmassf
  vmass=vmassi-fmassf
  fmassfm=fmassf
  IF(fuel <= 0.) THEN
    mprop=1
    thrust=0.
    fmassfd=0.
    fmassf=0.
  END IF
END IF

thrustx=thrust/1000.

RETURN
END SUBROUTINE a2
!**********************************************************************

SUBROUTINE a3
!**********************************************************************
!*** * Force Module A3
!*** * Reserved C(3510) locations are 1400-1499
!*** * This module performs the following functions:
!*** *
!*** * Calculates the specific force acting on the vehicle
!*** *
!*** * MODIFICATION HISTORY
!*** * 960701  Created by Peter Zipfel
!*** *
!*** ******************************************************************

COMMON c(3510)

DIMENSION fapm(3),fspv(3)

!*** INPUT DATA

EQUIVALENCE (c(1401),sref)
EQUIVALENCE (c(1402),phimvx)

! SREF = D Aerodynamic reference area - m^2
! PHIMVX = D Bank angle of maneuver plane wrt vertical - deg


!*** INPUT FROM EXECUTIVE

EQUIVALENCE (c(0052),crad)

!*** INPUT FROM OTHER MODULES

EQUIVALENCE (c(0207),pdynmc)
EQUIVALENCE (c(1201),cd)
EQUIVALENCE (c(1202),cl)
EQUIVALENCE (c(1203),alphax)
EQUIVALENCE (c(1309),vmass)
EQUIVALENCE (c(1313),thrustx)
EQUIVALENCE (c(1613),dvbe)

! PDYNMC= G Dynamic Pressure - Pa
! CD= O Drag coefficient - ND
! CL= O Lift coefficient - ND
! ALPHAX= D/O Angle of attack - deg
! VMASS= I/O Vehicle mass - kg
! THRUSTX= O Thrust at altitude - kN
! DVBE= I/G Geographic speed - m/s

!*** OUTPUTS TO OTHER MODULES

EQUIVALENCE (c(1405),fspv(1))

! FSPV(3) = O Specific force in velocity coordinates - m/s^2

!*** DIAGNOSTICS

EQUIVALENCE (c(1403),fd)
EQUIVALENCE (c(1404),fl)

! FD = G Drag force on vehicle - N
! FL = G Lift force on vehicle - N
! PDYNMC = G Dynamic Pressure - Pa

!*** CALCULATE AERODYNAMIC FORCES

fd=pdynmc*sref*cd
fl=pdynmc*sref*cl

!*** CALCULATE NON-GRAVITATIONAL FORCES IN MANEUVER PLANE

fapm(1)=-fd+thrustx*1000.*COS(alphax/crad)
fapm(2)=0.
fapm(3)=-(fl+thrustx*1000.*SIN(alphax/crad))

!*** SPECIFIC FORCE IN VELOCITY AXES

fspv(1)=fapm(1)/vmass
fspv(2)=-SIN(phimvx/crad)*fapm(3)/vmass
fspv(3)=COS(phimvx/crad)*fapm(3)/vmass

RETURN
END SUBROUTINE a3
!**********************************************************************

SUBROUTINE g2
!**********************************************************************
!*** *
!*** * Atmosphere and gravity module in SI units
!*** * Reserved C(3510) locations are 200-299
!*** * This module performs the following functions:
!*** * 1) Calculates the atmospheric properties
!*** * 2) Calculates the gravitational acceleration
!*** * 3) Calculates the vehicle Mach number and dynamic pressure
!*** * 4) Inputs special weather deck from INPUT.ASC
!*** *
!*** * MAIR=0 International Standard Atmosphere ISO 1962
!*** *     =1 Weather Deck (Atmophere only, wind not used in 3 DoF sims)
!*** * COMMOM /WINDS/ read in from INPUT.ASC WEATHER deck
!*** *  (OPTMET=1 required, SI units)
!*** *  WALT= Altitude - m
!*** *  WDIR= Wind Direction (from North) - deg
!*** *  WVEL= Wind Velocity - m/s
!*** *  RHX= Air density - kg/m^3
!*** *  CTMP= Temprature - deg Celsius
!*** *  WPRES= Atmospheric pressure - Pa
!*** *  KOUNTW= Number of altitude records
!*** *  RHW= Last altitude record
!*** *
!*** * MODIFICATION HISTORY
!*** * 931007 Created by Peter Zipfel
!*** *
!*** ******************************************************************

COMMON c(3510)
COMMON /winds/walt(50),wdir(50),wvel(50),rhx(50),  &
    ctmp(50),wpres(50),kountw,rhw

!*** INPUT DATA

EQUIVALENCE (c(0200),mair)

! MAIR = D =0:Std Atmosphere, =1: Weather Deck

!*** INPUT FROM EXECUTIVE ROUTINE

EQUIVALENCE (c(0051),rearth)

! REARTH = E Radius of Earth - m

!*** INPUT FROM OTHER MODULES

EQUIVALENCE (c(1606),balt)
EQUIVALENCE (c(1613),dvbe)

! BALT= I/O Vehicle altitude = m
! DVBE= I/G Geographic speed - m/s

!*** OUTPUT TO OTHER MODULES

EQUIVALENCE (c(0202),patm)
EQUIVALENCE (c(0203),rho)
EQUIVALENCE (c(0205),grav)
EQUIVALENCE (c(0206),vmach)
EQUIVALENCE (c(0207),pdynmc)

! PATM = O Atmospheric pressure - Pa
! RHO = O Atmospheric density - kg/m^3
! GRAV = O Gravity acceleration - m/s^2
! VMACH = O Mach number of rocket - ND
! PDYNMC = O Dynamic pressure - Pa

!*** DIAGNOSTICS

EQUIVALENCE (c(0201),tempk)
EQUIVALENCE (c(0204),vsound)

! TEMPK = G Atmospheric temperature - degK
! VSOUND = G Sonic speed - m/sec


!*** PARAMETERS

PARAMETER (g=6.673E-11)
PARAMETER (r=287.053)
PARAMETER (emass=5.973E24)

! G =Gravitaional constant - N*m^2/kg^2
! R =Gas constant - m^2/(K*sec^2
! EMASS =Mass of earth - kg

!*** ALTITUDE ABOVE CENTER OF EARTH

rad = rearth + balt

!*** CALCULATE THE GRAVITY ACCELERATION

grav=g*emass/rad**2

!*** CALCUL THE ATMOSPH DENSITY, SONIC SPEED AND ROCKET MACH NUMBER

IF(mair == 0) THEN
  IF(balt < 11000.)THEN
    tempk=288.15-0.0065*balt
    patm=101325.*(tempk/288.15)**5.2559
  ELSE
    tempk=216.
    patm=22630.*EXP(-0.00015769*(balt-11000.))
  END IF
  
  rho=patm/(r*tempk)
ELSE
  CALL table(balt,walt,rhx,kountw,rho)
  CALL table(balt,walt,ctmp,kountw,ctemp)
  CALL table(balt,walt,wpres,kountw,patm)
  tempk=ctemp+273.16
END IF

vsound=SQRT(1.4*r*tempk)

vmach=ABS(dvbe/vsound)

pdynmc=rho*dvbe**2/2.

RETURN
END SUBROUTINE g2
!**********************************************************************

SUBROUTINE d1i
!*** ******************************************************************
!*** * Initializes the equations of motions of Module D1
!*** * Reserved C(3510) locations are 1600-1699
!*** * This module performs the following functions
!*** *
!*** * 1) Define the locations of the state and state derivative
!*** *    variables
!*** * 2) Converts geographic inputs into inertial coordinates
!*** *
!*** * MODIFICATION HISTORY
!*** * 960711 Created by Peter Zipfel
!*** *
!*** ******************************************************************

COMMON c(3510)

DIMENSION ipl(100),iplv(100),vbeg(3),tge(3,3),teg(3,3)  &
    ,sbie(3),tvg(3,3),tgv(3,3),tie(3,3),sbii(3),tig(3,3),weii(3,3)  &
    ,dum3(3),vbei(3),vbii(3)

!*** INPUT DATA INITIALIZATION

EQUIVALENCE (c(1602),psivgx)
EQUIVALENCE (c(1603),thtvgx)
EQUIVALENCE (c(1604),blon)
EQUIVALENCE (c(1605),blat)
EQUIVALENCE (c(1606),balt)
EQUIVALENCE (c(1610),baltft)
EQUIVALENCE (c(1613),dvbe)

! PSIVGX = I Heading angle from north - deg
! THTVGX = I Flight path angle from horizontal - deg
! BLON = I/G Vehicle longitude - rad
! BLAT = I/G Vehicle latitude - rad
! BALT = I/O Vehicle altitude - m
! BALTFT = I/O Vehicle altitude - ft
! DVBE = I/G Geographic speed - m/s

!*** INPUT FROM EXECUTIVE

EQUIVALENCE (c(0051),rearth)
EQUIVALENCE (c(0052),crad)
EQUIVALENCE (c(0058),weii3)
EQUIVALENCE (c(2562),ipl(1))
EQUIVALENCE (c(2867),iplv(1))
EQUIVALENCE (c(2561),nip)

! IPL(100) = E State derivitave c-array locations
! IPLV(100) = E State c-array locations
! N = E Number of variables to integrate

!*** INITIALIZATION

EQUIVALENCE (c(1622),tgv(1,1))
EQUIVALENCE (c(1631),tig(1,1))
EQUIVALENCE (c(1649),sbii(1))
EQUIVALENCE (c(1643),vbii(1))
EQUIVALENCE (c(1658),balt0)

!***  INITIALIZATION OF STATE VARIABLES

iloc=1640
DO i=0,2
  ipl(nip)=iloc+i
  iplv(nip)=iloc+i+3
  nip=nip+1
END DO

iloc=1646
DO i=0,2
  ipl(nip)=iloc+i
  iplv(nip)=iloc+i+3
  nip=nip+1
END DO

!***INPUT CONVERSION TO SBII AND VBII AND INITIAL TGV AND TIG

sbie(1)=(balt+rearth)*COS(blat)*COS(blon)
sbie(2)=(balt+rearth)*COS(blat)*SIN(blon)
sbie(3)=(balt+rearth)*SIN(blat)
CALL matuni(tie,3)
CALL matmul(sbii,tie,sbie,3,3,1)

psivg=psivgx/crad
thtvg=thtvgx/crad
CALL matcar(vbeg,dvbe,psivg,thtvg)
CALL cadtge3(tge,blon,blat)
CALL mattra(teg,tge,3,3)
CALL matzer(weii,3,3)
weii(1,2)=-weii3
weii(2,1)=weii3
CALL matmul(dum3,weii,sbii,3,3,1)
CALL matmul(tig,tie,teg,3,3,3)
CALL matmul(vbei,tig,vbeg,3,3,1)
CALL matadd(vbii,vbei,dum3,3,1)
CALL mat2tr(tvg,psivg,thtvg)
CALL mattra(tgv,tvg,3,3)

!*** SAVE LAUNCH ALTITUDE

balt0=balt
baltft=balt*3.2808399

RETURN
END SUBROUTINE d1i
!*******************************************************************

SUBROUTINE d1
!*******************************************************************
!*** * Equations of motion Module D1
!*** * Cartesian inertial form, round rotating earth
!*** * Reserved C(3510) locations are 1600-1699
!*** * This module performs the following functions
!*** *
!*** * 1) Solves Newton's Law for spherical rotating earth in
!*** *    inertial coordinates
!*** * 2) Converts output to geographic variables
!*** *
!*** * MODIFICATION HISTORY
!*** * 960711 Created by Peter Zipfel
!*** *
!*** **************************************************************

COMMON c(3510)

DIMENSION fspg(3),fspv(3),agravg(3),ai(3),tig(3,3),tei(3,3)  &
    ,sbie(3),sbii(3),vbei(3),tge(3,3),tgi(3,3),tvg(3,3)  &
    ,tgv(3,3),weii(3,3),vbig(3),vbii(3),vbiid(3) ,sbiid(3),dum3(3),vbeg(3),accg(3)

!*** INPUT FROM EXECUTIVE

EQUIVALENCE (c(0051),rearth)
EQUIVALENCE (c(0052),crad)
EQUIVALENCE (c(0058),weii3)
EQUIVALENCE (c(2000),t)

! CRAD = E Conversion from radians to degrees = 57.298
! WEII3 = E Earth rotation - rad/sec

!*** INITIALIZATION

EQUIVALENCE (c(1622),tgv(1,1))
EQUIVALENCE (c(1631),tig(1,1))
EQUIVALENCE (c(1658),balt0)

! TGV(3,3) = I T.M. of  geographic wrt velocity coord - ND
! TIG(3,3) = I T.M. of inertial wrt geographic coord - ND
! BALT0 = I Saved value of initial altitude - m

!***  INPUT FROM OTHER MODULES

EQUIVALENCE (c(0205),grav)
EQUIVALENCE (c(1405),fspv(1))

! GRAV= O Gravity acceleration - m/s^2
! FSPV= O Specific force in velocity coordinates - m/s^2

!*** STATE VARIABLES

EQUIVALENCE (c(1640),vbiid(1))
EQUIVALENCE (c(1643),vbii(1))
EQUIVALENCE (c(1646),sbiid(1))
EQUIVALENCE (c(1649),sbii(1))

! VBIID(3) = S Time derivative of VBII(3) - m/s^2
! VBII(3) = S Vel of missile wrt inertial frame in inertial axes - m
! SBIID(3) = S Time derivative of SBIE(3) - m/s
! SBII(3) = S Missile displacement from earth center in inertial axes - m

!*** OUTPUT TO OTHER MODULES

EQUIVALENCE (c(1606),balt)
EQUIVALENCE (c(1613),dvbe)

! BALT = O Vehicle altitude = m
! DVBE = I/O Geographic speed - m/s

!*** DIAGNOSTICS

EQUIVALENCE (c(1602),psivgx)
EQUIVALENCE (c(1603),thtvgx)
EQUIVALENCE (c(1604),blon)
EQUIVALENCE (c(1605),blat)
EQUIVALENCE (c(1607),dvbi)
EQUIVALENCE (c(1608),psivigx)
EQUIVALENCE (c(1609),thtvigx)
EQUIVALENCE (c(1610),baltft)
EQUIVALENCE (c(1652),vbeg(1))
EQUIVALENCE (c(1655),vbig(1))

! PSIVGX = G Heading angle from north - deg
! THTVGX = G Flight path angle from horizontal - deg
! BLON = G Vehicle longitude - rad
! BLAT = G Vehicle latitude - rad
! DVBI = G Speed of vehicle wrt inertial frame
! PSIVIGX = G Heading angle of inertial vel vect - deg
! THTVIGX = G Flight path angle of inert vel vec  - deg
! BALTFT = G Vehicle Altitude - ft
! VBEG(3) = G Geographic velocity in geographic coord - m/s
! VBIG(3) = G Inertial velocity in geographic coord - m/s

!*** RIGHT HAND SIDE OF DYNAMIC EQUATIONS

CALL matmul(fspg,tgv,fspv,3,3,1)

CALL vecvec(agravg,0.,0.,grav)

CALL matadd(accg,fspg,agravg,3,1)

CALL mateql(dum3,accg,3,1)

CALL matmul(ai,tig,dum3,3,3,1)

!*** STATE VARIABLE INTEGRATION

CALL mateql(vbiid,ai,3,1)
CALL mateql(sbiid,vbii,3,1)

!*** UPDATE LONGITUDE, LATITUDE AND ALTITUDE, TVG AND FLIGHT PATH ANGLES

CALL cadtei3(tei)
CALL matmul(sbie,tei,sbii,3,3,1)
CALL cadsph3(blon,blat,balt,dbi,sbie)
CALL cadtge3(tge,blon,blat)

CALL matzer(weii,3,3)
weii(1,2)=-weii3
weii(2,1)=weii3
CALL matmul(dum3,weii,sbii,3,3,1)
CALL matsub(vbei,vbii,dum3,3,1)
CALL matmul(tgi,tge,tei,3,3,3)
CALL matmul(vbeg,tgi,vbei,3,3,1)
CALL matpol(dvbe,psivg,thtvg,vbeg)
psivgx=psivg*crad
thtvgx=thtvg*crad

!*** FOR NEXT INTEGRATION CYCLE: TIG, TGV

CALL mattra(tig,tgi,3,3)
CALL mat2tr(tvg,psivg,thtvg)
CALL mattra(tgv,tvg,3,3)

!*** DIAGNOSTIC: INERTIAL VELOCITY IN GEOGRAPHIC AXES

CALL matmul(vbig,tgi,vbii,3,3,1)
CALL matpol(dvbi,psivig,thtvig,vbig)
psivigx=psivig*crad
thtvigx=thtvig*crad
!---
!-- Is this the right spot???
!-- Save the altitude in feet.
baltft=balt*3.2808399

RETURN
END SUBROUTINE d1

!*******************************************************************

SUBROUTINE cadsph3(blon,blat,balt,dbi,sbie)
!*** ***************************************************************
!*** * Calculates longitude, latitude and altitude from earth position
!*** *
!*** * Argument Output:
!*** *          BLON =Missile longitude - rad
!*** *          BLAT =Missile latitude - rad
!*** *          BALT =Missile altitude above sea level - rad
!*** *          DBI =Missile distance from earth center - m
!*** * Argument Input:
!*** *          SBIE(3) =Missile position wrt earth center in earth coor - m
!*** *
!*** * MODIFICATION HISTORY
!*** * 960628 Created by Peter Zipfel
!*** * 000128 Resolved multivalued ARCSIN function, PZi
!*** *
!*** ***************************************************************

REAL, INTENT(OUT)                        :: blon
REAL, INTENT(OUT)                        :: blat
REAL, INTENT(OUT)                        :: balt
REAL, INTENT(OUT)                        :: dbi
REAL, INTENT(IN)                         :: sbie(3)
COMMON  c(3510)



!*** INPUT FROM EXEC

EQUIVALENCE (c(0051),rearth)
EQUIVALENCE (c(0052),crad)

!*** LATITUDE

dbi=SQRT(sbie(1)**2+sbie(2)**2+sbie(3)**2)
dum1=sbie(3)/dbi
IF(ABS(dum1) > 1.) dum1=SIGN(1.,dum1)
blat=ASIN(dum1)

!*** ALTITUDE

balt=dbi-rearth
baltft=balt*3.2808399

!*** LONGITUDE

dum3=sbie(2)/SQRT(sbie(1)**2+sbie(2)**2)
IF(ABS(dum3) > 1.) dum3=SIGN(1.,dum3)
dum4=ASIN(dum3)

!*** RESOLVING THE MUTLIVALUED ARCSIN FUNCTION

IF(sbie(1) >= 0..AND.sbie(2) >= 0.) alamda=dum4 !1. quadrant
IF(sbie(1) < 0..AND.sbie(2) >= 0.) alamda=180./crad-dum4 !2. quadrant
IF(sbie(1) < 0..AND.sbie(2) < 0.) alamda=180./crad-dum4 !3. quadrant
IF(sbie(1) >= 0..AND.sbie(2) < 0.) alamda=360./crad+dum4 !4. quadrant
blon=alamda
IF(blon > (180./crad)) blon=-((360./crad)-blon) !east pos., west neg.

RETURN
END SUBROUTINE cadsph3
!*******************************************************************

SUBROUTINE cadtge3(tge,blon,blat)
!*** ***************************************************************
!*** * Calculates transformation matrix TGE from longitude and latitude
!*** *
!*** * Argument Output:
!*** *          TGE(3,3) =Transf. of geographic wrt earth coor
!*** * Argument Input:
!*** *          BLON =Missile longitude - rad
!*** *          BLAT =Missile latitude - rad
!*** *
!*** * MODIFICATION HISTORY
!*** * 960628 Created by Peter Zipfel
!*** *
!*** ***************************************************************


REAL, INTENT(OUT)                        :: tge(3,3)
REAL, INTENT(IN OUT)                     :: blon
REAL, INTENT(IN OUT)                     :: blat
COMMON  c(3510)



slon=SIN(blon)
clon=COS(blon)
slat=SIN(blat)
clat=COS(blat)
tge(1,1)=-slat*clon
tge(1,2)=-slat*slon
tge(1,3)=clat
tge(2,1)=-slon
tge(2,2)=clon
tge(2,3)=0.
tge(3,1)=-clat*clon
tge(3,2)=-clat*slon
tge(3,3)=-slat

RETURN
END SUBROUTINE cadtge3
!*******************************************************************

SUBROUTINE cadtei3(tei)
!*** ***************************************************************
!*** * Calculates transformation matrix TIE from time and WEII3
!*** *
!*** * Argument Output:
!*** *          TEI(3,3) =Transf. of inertial wrt earth coor
!*** *
!*** * MODIFICATION HISTORY
!*** * 960711 Created by Peter Zipfel
!*** *
!*** ***************************************************************


REAL, INTENT(OUT)                        :: tei(3,3)
COMMON  c(3510)



!*** INPUT FROM EXECUTIVE

EQUIVALENCE (c(0058),weii3)
EQUIVALENCE (c(2000),t)

xi=weii3*t
sxi=SIN(xi)
cxi=COS(xi)
CALL matuni(tei,3)
tei(1,1)=cxi
tei(1,2)=sxi
tei(2,1)=-sxi
tei(2,2)=cxi

RETURN
END SUBROUTINE cadtei3
!********************* DUMMY RETURNS **********************************

SUBROUTINE a3i
RETURN
END SUBROUTINE a3i

SUBROUTINE a4i
RETURN
END SUBROUTINE a4i

SUBROUTINE a4
RETURN
END SUBROUTINE a4

SUBROUTINE a5i
RETURN
END SUBROUTINE a5i

SUBROUTINE a5
RETURN
END SUBROUTINE a5

SUBROUTINE c1i
RETURN
END SUBROUTINE c1i

SUBROUTINE c1
RETURN
END SUBROUTINE c1
!      SUBROUTINE C2I
!      RETURN
!      END
!      SUBROUTINE C2
!      RETURN
!      END

SUBROUTINE c3i
RETURN
END SUBROUTINE c3i

SUBROUTINE c3
RETURN
END SUBROUTINE c3
!       SUBROUTINE C4I
!      RETURN
!      END
!      SUBROUTINE C4
!      RETURN
!      END

SUBROUTINE c5i
RETURN
END SUBROUTINE c5i

SUBROUTINE c5
RETURN
END SUBROUTINE c5

SUBROUTINE d2i
RETURN
END SUBROUTINE d2i

SUBROUTINE d2
RETURN
END SUBROUTINE d2

SUBROUTINE d3i
RETURN
END SUBROUTINE d3i

SUBROUTINE d3
RETURN
END SUBROUTINE d3

SUBROUTINE d4
RETURN
END SUBROUTINE d4

SUBROUTINE d4i
RETURN
END SUBROUTINE d4i

SUBROUTINE d5i
RETURN
END SUBROUTINE d5i

SUBROUTINE d5
RETURN
END SUBROUTINE d5

SUBROUTINE g1i
RETURN
END SUBROUTINE g1i

SUBROUTINE g1
RETURN
END SUBROUTINE g1

SUBROUTINE g2i
RETURN
END SUBROUTINE g2i

SUBROUTINE g3i
RETURN
END SUBROUTINE g3i

SUBROUTINE g3
RETURN
END SUBROUTINE g3

SUBROUTINE g4i
RETURN
END SUBROUTINE g4i

SUBROUTINE g4
RETURN
END SUBROUTINE g4

SUBROUTINE g5i
RETURN
END SUBROUTINE g5i

SUBROUTINE g5
RETURN
END SUBROUTINE g5

SUBROUTINE s1i
RETURN
END SUBROUTINE s1i

SUBROUTINE s1
RETURN
END SUBROUTINE s1

SUBROUTINE s2i
RETURN
END SUBROUTINE s2i

SUBROUTINE s2
RETURN
END SUBROUTINE s2

SUBROUTINE s3i
RETURN
END SUBROUTINE s3i

SUBROUTINE s3
RETURN
END SUBROUTINE s3

SUBROUTINE s4i
RETURN
END SUBROUTINE s4i

SUBROUTINE s4
RETURN
END SUBROUTINE s4

SUBROUTINE s5i
RETURN
END SUBROUTINE s5i

SUBROUTINE s5
RETURN
END SUBROUTINE s5
!**********************************************************************

SUBROUTINE c2i
!**********************************************************************
!*** * Autopilot Initialization Module
!*** * Reserved C(3510) locations are 900-999
!*** *
!*** * This module performs the following functions:
!*** * (1) Initializes the state variables
!*** *
!*** * MODIFICATION HISTORY
!*** * 13-07-2012 T Jelliffe    Added empty functions to module
!*** *
!**********************************************************************

COMMON c(3510)

DIMENSION ipl(100),iplv(100)

!*** INPUT FROM EXECUTIVE

EQUIVALENCE (c(2561),nip)
EQUIVALENCE (c(2562),ipl(1))
EQUIVALENCE (c(2867),iplv(1))

!*** STORAGE OF STATE VARIABLE LOCATIONS

iloc=914
DO i=1,2
  ipl(nip)=iloc
  iplv(nip)=iloc+1
  iloc=iloc+2
  nip=nip+1
END DO

PRINT *,"Autopilot Init"
RETURN
END SUBROUTINE c2i
!**********************************************************************

SUBROUTINE c2
!**********************************************************************
!*** * Autopilot Module
!*** * Reserved C(3510) locations are 900-999
!*** *
!*** * MODIFICATION HISTORY
!*** * 13-07-2012 T Jelliffe   Added in the empty functions
!*** *
!**********************************************************************

COMMON c(3510)

!*** INPUT FROM EXECUTIVE

EQUIVALENCE (c(0052),crad)
EQUIVALENCE (c(2000),t)

!*** INPUT DATA


! -- Commented out to compile
!      EQUIVALENCE (C(0900),MAUT)

! MAUT = D MAUT=|MAUTY|MAUTP|,see table in Module C2

!*** INPUT FROM OTHER MODULES


! -- Commented out to compile
!      EQUIVALENCE (C(0802),ANCOMX)
!      EQUIVALENCE (C(0803),AYCOMX)


! ANCOMX= O Normal acceleration command - g's
! AYCOMX= O Yaw acceleration command - g's

!*** OUTPUT TO OTHER MODULES


!      EQUIVALENCE (C(0919),DELACX)
!      EQUIVALENCE (C(0920),DELECX)
!      EQUIVALENCE (C(0921),DELRCX)


! DELACX = O Aileron command - deg
! DELECX = O Elevator command - deg
! DELRCX = O Rudder command - deg

!*** DIAGNOSTICS

!*** Call yaw stability augmentation system autopilot

RETURN
END SUBROUTINE c2
!**********************************************************************

SUBROUTINE c4i
!**********************************************************************
!*** * Actuator Initialization Module
!*** * Reserved C(3510) locations are 900-999
!*** *
!*** * This module performs the following functions:
!*** * (1) Initializes the state variables
!*** *
!*** * MODIFICATION HISTORY
!*** * 13-07-2012 T Jelliffe    Added empty functions to module
!*** *
!**********************************************************************

COMMON c(3510)


PRINT *,"Actuator Init"
RETURN
END SUBROUTINE c4i
!**********************************************************************

SUBROUTINE c4
!**********************************************************************
!*** * Actuator Module
!*** * Reserved C(3510) locations are 900-999
!*** *
!*** * MODIFICATION HISTORY
!*** * 13-07-2012 T Jelliffe   Added in the empty functions
!*** *
!**********************************************************************

COMMON c(3510)

!*** INPUT FROM EXECUTIVE

EQUIVALENCE (c(0052),crad)
EQUIVALENCE (c(2000),t)

!*** DIAGNOSTICS

EQUIVALENCE (c(1602),psivgx)
EQUIVALENCE (c(1603),thtvgx)
EQUIVALENCE (c(1604),blon)
EQUIVALENCE (c(1605),blat)
EQUIVALENCE (c(1607),dvbi)
EQUIVALENCE (c(1608),psivigx)
EQUIVALENCE (c(1609),thtvigx)
EQUIVALENCE (c(1610),baltft)
EQUIVALENCE (c(1203),alphax)

! PSIVGX = G Heading angle from north - deg
! THTVGX = G Flight path angle from horizontal - deg
! BLON = G Vehicle longitude - rad
! BLAT = G Vehicle latitude - rad
! DVBI = G Speed of vehicle wrt inertial frame
! PSIVIGX = G Heading angle of inertial vel vect - deg
! THTVIGX = G Flight path angle of inert vel vec  - deg
! BALTFT = G Vehicle Altitude - ft

!*** INPUT FROM OTHER MODULES

EQUIVALENCE (c(1606),balt)
EQUIVALENCE (c(1613),dvbe)
EQUIVALENCE (c(0900),orbalt)

! BALT= I/O Vehicle altitude = m
! DVBE= I/G Geographic speed - m/s
! ORBALT = I Target orbital altitude - m

!*** OUTPUT TO OTHER MODULES

!      EQUIVALENCE (C(xx),xxx)

! ALPHAX = O Angle of attack - deg

!      PRINT *,"Orbital Altitude:", ORBALT
IF (t > 60.0) thtvgx = 88.
IF (t > 80.0) thtvgx = 85.
IF (t > 90.0) thtvgx = 80.

!      PRINT *,"THTVGX = :", THTVGX
RETURN
END SUBROUTINE c4
